双臂协同避障搬运使用说明

[TOC]

1. 功能概述

本文档说明如何在已有双臂协同 speedL 示教与 moveL 搬运基础上，增加附加工件和环境障碍物，实现双臂夹持同一刚性对象时的协同避障搬运。

本功能面向用户实际使用流程：协同对象起点、终点由用户通过 speedL 或 speedJ 示教获得，不在 Demo 中手动构造对象位姿或调用逆解生成目标点。

整体流程如下：

建立双臂协同场景（单 Planner，MASTER + SLAVE）
                    ↓
夹持协同对象，以实际关节反馈初始化双臂并进入 IDEL
                    ↓
用户通过 speedL/speedJ 示教到起点，停稳后记录 PathPoint
                    ↓
用户通过 speedL/speedJ 示教到终点，停稳后记录 PathPoint
                    ↓
mdlSetAttachedLink()：把被搬运工件附加到机械臂连杆
                    ↓
utlSetWorldObject()：设置环境障碍物
                    ↓
tpGenerateCollisionFreePoints()：生成协同对象无碰绕行路点
                    ↓
同步 moveJ 到第一个输出点（起点），停稳后重新进入 IDEL
                    ↓
从第二个输出点开始逐点添加协同 moveL 并执行
                    ↓
清理：utlUnsetWorldObject() + mdlRemoveAttachedLink()

tpGenerateCollisionFreePoints() 会在规划阶段检查附加工件、机器人和环境障碍物之间的碰撞。Demo 的取点和执行循环不再额外调用碰撞检测接口，避免在高频取点过程中引入不必要耗时。

本文面向双臂笛卡尔协同避障。双臂关节同步避障属于另一种规划组织方式，见双臂/外部轴关节同步避障使用说明。

2. 使用约束

2.1 前置条件

使用本文流程前，应先满足第 8.3 节的双臂协同搬运要求：

• 两台机械臂在同一个 Scene 中，并通过 mdlSetBasePoseInWorld() 设置现场实际基座位姿；
• 使用同一个 Planner，主、从臂分别设为 PlannerRole::MASTER 和 PlannerRole::SLAVE；
• 协同逆解策略使用 IKStrategy::ROBOTIC_ARM_IK_STRATEGY；
• ToolWorkpiece 中主臂 type/tool/payload 与从臂 type_s/tool_s/payload_s 配置完整；
• 两臂已经夹持同一刚性对象，并以实际关节反馈初始化 State，进入 TaskState::IDEL 建立双臂固定变换；
• 当前双臂协同运动不支持远端工具，type 和 type_s 应均设为 0。

2.2 示教点要求

避障规划输入的起点和终点必须来自实际示教记录。每个示教点应满足：

• type = DescribeSpace::FULL；
• pose 为协同对象位姿；
• joint_m 为主臂关节构型；
• joint_s 为从臂关节构型；
• t_w 包含完整主、从工具工件信息；
• ik_strategy = IKStrategy::ROBOTIC_ARM_IK_STRATEGY。

speedL 适合按协同对象笛卡尔方向示教；speedJ 适合按关节方向微调到目标姿态。无论使用哪种方式，记录点时都以停稳后的实际轨迹点或实际关节反馈为准，不手动构造目标位姿。

2.3 附加工件与环境障碍物

• 被搬运工件通过 mdlSetAttachedLink() 附加到主臂或从臂的指定连杆上，后续跟随该连杆运动并参与避障规划。
• 环境障碍物通过 utlSetWorldObject() 放入世界环境，表示当前规划需要避开的静态物体。
• touch_links 用于忽略工件与夹持连杆之间的正常接触，应只填写确实允许接触的连杆。

2.4 安全前提

• 首次运行应降低速度和加速度，在模拟环境或低风险环境中验证。
• 示教与执行阶段仍需由上层控制系统监测关节限位、速度、力矩、夹持状态和现场安全信号。
• tpGenerateCollisionFreePoints() 与 tpAddPositionLine() 的返回值必须检查，失败时不得继续执行。
• 避障规划不替代控制器安全功能和现场急停回路。

3. 接口说明

3.1 附加工件

/**
 * @brief 设置附加到机器人连杆上的碰撞物体；同名附加连杆已存在时进行替换。
 *        双臂协同避障搬运中通常用于把被搬运工件附加到主臂或从臂法兰/工具连杆上，
 *        使工件跟随父连杆运动并参与后续碰撞检测和避障规划。
 * @param parent_link_name 附加连杆所依附的机器人连杆名称。
 * @param attached_link_name 附加连杆名称。该名称用于后续替换或删除附加工件。
 * @param shapes 附加连杆包含的碰撞几何体。
 * @param relative_poses 各碰撞几何体相对于父连杆的位姿，与 shapes 一一对应。
 * @param touch_links 默认不与该附加连杆进行碰撞及距离检测的机器人连杆名称。
 *        为空时不额外忽略任何机器人连杆。
 * @return 返回值 < 0 表示设置失败，具体定义查询 return_value_definition.hpp。
 */
virtual int mdlSetAttachedLink(
    const std::string& parent_link_name,
    const std::string& attached_link_name,
    const std::vector<geometric_shapes::ShapePtr>& shapes,
    const std::vector<RLPose>& relative_poses,
    const std::vector<std::string>& touch_links) = 0;

/**
 * @brief 删除指定名称的附加连杆。
 *        搬运结束、工件释放或需要重新设置工件模型时调用。
 * @param attached_link_name 需要删除的附加连杆名称，应与 mdlSetAttachedLink() 中填写的
 *        attached_link_name 一致。
 * @return 返回值 < 0 表示删除失败，具体定义查询 return_value_definition.hpp。
 */
virtual int mdlRemoveAttachedLink(const std::string& attached_link_name) = 0;

relative_poses 是工件碰撞几何相对父连杆的位姿。夹持后工件与法兰或工具存在正常接触时，可在 touch_links 中填写对应连杆，避免规划阶段误报接触碰撞。

3.2 环境障碍物

/**
 * @brief 添加物体到世界环境；object_id 已存在时进行替换。
 *        双臂协同避障搬运中用于设置工作台、立柱、料框等静态环境障碍物。
 * @param object_id 物体名称。该名称用于后续移动、替换或删除世界物体。
 * @param pose 物体坐标系相对于世界坐标系的位姿。
 * @param shapes 物体包含的形状集合。
 * @param origins 各形状相对于物体坐标系的位姿，与 shapes 一一对应。
 * @return 返回值 < 0 表示设置失败，具体定义查询 return_value_definition.hpp。
 */
virtual int utlSetWorldObject(
    const std::string& object_id,
    const RLPose& pose,
    const std::vector<geometric_shapes::ShapeConstPtr>& shapes,
    const std::vector<RLPose>& origins) = 0;

/**
 * @brief 删除世界环境中的指定物体。
 *        搬运结束或场景切换时调用，避免旧障碍物继续影响后续规划。
 * @param object_id 需要删除的物体名称，应与 utlSetWorldObject() 中填写的 object_id 一致。
 * @return 返回值 < 0 表示删除失败，具体定义查询 return_value_definition.hpp。
 */
virtual int utlUnsetWorldObject(const std::string& object_id) = 0;

pose 描述物体坐标系在世界坐标系下的位姿，origins 描述各形状在物体坐标系下的位姿。常用写法如下：

// 物体坐标系位于世界某处，形状位于物体坐标系原点。
utility->utlSetWorldObject("obstacle", obstacle_world_pose, {box}, {{0, 0, 0, 0, 0, 0}});

// 物体坐标系位于世界原点，形状直接使用世界位姿。
utility->utlSetWorldObject("obstacle", {}, {box}, {obstacle_world_pose});

3.3 无碰路径生成

/**
 * @brief 规划生成无碰撞路径。
 *        双臂协同避障场景下，输入点描述协同对象路径，规划器会结合主、从臂当前固定变换、
 *        附加工件和世界障碍物生成无碰路点。
 * @param in_points 描述路径几何信息的 N 个路径点，N >= 2。
 *        双臂协同避障输入点需为示教获得的 FULL 点，并填写 pose、joint_m、joint_s、t_w
 *        和 ik_strategy。
 * @param path_property 路径几何属性。双臂协同避障需设置 describe_space = CARTESIAN，
 *        cur_prop.type = LINE。
 * @param obstacle_avoidance_params 避障属性，包括避障算法类型、规划步长以及是否使能路径简化。
 * @param out_points 规划输出的无碰撞路径点。输出点包含起点、终点和必要的中间绕行点。
 * @return 返回值 < 0 表示规划失败，具体定义查询 return_value_definition.hpp。
 */
virtual int tpGenerateCollisionFreePoints(
    const std::vector<PathPoint>& in_points,
    const PathProperty& path_property,
    const ObstacleAvoidanceParameters& obstacle_avoidance_params,
    std::vector<PathPoint>& out_points) = 0;

双臂协同避障应设置：

path_property.describe_space = DescribeSpace::CARTESIAN;
path_property.cur_prop.type = CurveType::LINE;

输入起终点必须是示教获得的 FULL 点。若直线可达且无碰撞，输出可为直线路点；若直线被障碍物阻断，规划器会搜索绕行路点。

4. 使用流程

4.1 初始化双臂协同场景

按第 8.3 节建立 Scene、主从 Model、State、Controller、单 Planner 和 Task。两臂夹持工件后，使用当前实际关节反馈调用 rsInitiateRobotState()，再调用 tskSetTaskState(TaskState::IDEL) 建立双臂固定变换。

4.2 示教采集起终点

起点和终点都通过 speedL 或 speedJ 示教获得：

1. 示教到起点，发送 tskStop()，继续执行减速轨迹直到 tskUpdateCycle() 返回 E_PLN_EXEC_EMPTY；
2. 使用最后一组有效主、从轨迹点记录 PathPoint start；
3. 重新用当前实际关节反馈初始化并进入 IDEL；
4. 示教到终点，按同样方式记录 PathPoint goal；
5. 取点阶段只记录位姿和关节构型，不调用碰撞检测。

记录点时推荐直接填写避障规划需要的 FULL 点：

PathPoint start = last_master_point.point;
start.type = DescribeSpace::FULL;
start.joint_m = last_master_point.point.joint_m;
start.joint_s = last_slave_point.point.joint_m;
start.t_w = dual_tw;
start.ik_strategy = IKStrategy::ROBOTIC_ARM_IK_STRATEGY;

4.3 设置工件和障碍物

示教点记录完成后，再设置被搬运工件和环境障碍物：

auto workpiece_box = std::make_shared<geometric_shapes::Box>(0.4, 0.3, 0.2);
RLPose object_in_flange{0.0, 0.0, 0.2, 0.0, 0.0, 0.0};

ret = master_model->mdlSetAttachedLink(
    "flange", "payload_workpiece", {workpiece_box}, {object_in_flange}, {"tool0"});
if (ret < 0) return ret;

auto obstacle_box = std::make_shared<geometric_shapes::Box>(0.3, 0.3, 0.5);
RLPose obstacle_world{0.3, 0.2, 0.4, 0.0, 0.0, 0.0};  // 按现场障碍物实测位姿填写。

ret = utility->utlSetWorldObject("obstacle", {}, {obstacle_box}, {obstacle_world});
if (ret < 0) return ret;

4.4 生成无碰绕行路点

设置 ObstacleAvoidanceParameters 和笛卡尔直线路径属性后，调用 tpGenerateCollisionFreePoints({start, goal}, ...) 生成无碰路径。输入必须使用第 4.2 节示教得到的 FULL 起终点，输出为空或接口返回负值都应视为规划失败。

生成后应检查每个输出点仍为 DescribeSpace::FULL，并使用 IKStrategy::ROBOTIC_ARM_IK_STRATEGY。如需继续按协同对象路径执行，还应为输出点补齐或保持当前双臂固定变换 t_w。完整调用方式见第 5 节 Demo 的“生成无碰绕行路点”步骤。

4.5 执行协同 moveL

示教终点记录完成后，机器人通常停在终点。执行避障搬运前，应先将双臂同步运动到 free_points.front() 对应的起点关节构型，并用起点处的实际反馈重新进入 IDEL。随后从第二个无碰路点开始添加协同 moveL。

协同避障输出的多段 moveL 支持交融。blend_radius = {0.0, 0.0} 表示不交融，规划器将严格经过每个中间路点, 减速到零；当无碰路点较多或相邻路段较短时，实际执行可能表现为“一卡一卡”。支持设置适当的非零交融半径，使相邻路段连续过渡。

交融风险： 交融轨迹会偏离 tpGenerateCollisionFreePoints() 生成的原始无碰折线。交融半径过大时，圆滑后的轨迹可能过度靠近障碍物。应从较小半径开始，结合相邻路段长度和轨迹与障碍物之间的安全距离逐步调整，并对交融后的实际轨迹重新进行低速验证。若中间点必须精确经过，或轨迹与障碍物之间的安全距离不足，则该处应关闭交融。

执行循环仍按第 8.3 节的顺序组织：

State → Task → ReferenceTrajectory → Controller → 同周期下发

取点时按 TrajectoryPoint::id 路由到主、从臂，不依赖 points 的顺序。Demo 中不在该循环内重复调用碰撞检测。

4.6 清理场景

搬运完成后，删除本次任务添加的世界障碍物和附加工件，避免影响后续规划任务。完整清理代码见第 5 节 Demo 的“清理场景”步骤。

5. 完整 Demo

下面 Demo 接续第 8.3 节的示教流程：先按 8.3 的方式用 speedL 或 speedJ 记录起点和终点，再在本文 Demo 中设置工件、障碍物，生成无碰路点并执行避障搬运。

#include <aral/robot_scene.hpp>
#include <geometric_shapes/shapes.h>
#include <memory>
#include <vector>

using namespace ARAL::interface;

int main()
{
    constexpr double cycle = 0.005;
    int ret = E_PLN_OK;

    // ======================== 1. 创建场景与主从模型 ========================
    // 两台机械臂必须位于同一个 Scene 中，并使用现场实测基座安装位姿。
    auto scene = static_cast<Scene*>(CreateARALScene("dual_arm_avoid_transport"));
    if (!scene) return -1;

    const ModelHandle master_model_h = scene->rlCreateRobotModel("master_arm", "/path/to/master/model/");
    const ModelHandle slave_model_h = scene->rlCreateRobotModel("slave_arm", "/path/to/slave/model/");
    auto master_model = scene->rlGetRobotModel(master_model_h);
    auto slave_model = scene->rlGetRobotModel(slave_model_h);
    if (!master_model || !slave_model) return -1;

    // 按现场实测安装位姿填写。
    if ((ret = master_model->mdlSetBasePoseInWorld({0.0, 0.0, 0.0, 0.0, 0.0, 0.0})) < 0) return ret;
    if ((ret = slave_model->mdlSetBasePoseInWorld({0.5, 0.0, 0.0, 0.0, 0.0, 0.0})) < 0) return ret;

    // ======================== 2. 创建 State 并配置关节约束 ========================
    // 规划器会从 State 读取关节速度、加速度约束，首次调试建议保守设置。
    const StateHandle master_state_h = scene->rlCreateRobotState(master_model_h, "master_state");
    const StateHandle slave_state_h = scene->rlCreateRobotState(slave_model_h, "slave_state");
    auto master_state = scene->rlGetRobotState(master_state_h);
    auto slave_state = scene->rlGetRobotState(slave_state_h);
    if (!master_state || !slave_state) return -1;

    master_state->rsSetJointMaximumSpeed({2.0, 2.0, 2.0, 2.0, 2.0, 2.0});
    master_state->rsSetJointMaximumAcceleration({6.0, 6.0, 6.0, 6.0, 6.0, 6.0});
    slave_state->rsSetJointMaximumSpeed({2.0, 2.0, 2.0, 2.0, 2.0, 2.0});
    slave_state->rsSetJointMaximumAcceleration({6.0, 6.0, 6.0, 6.0, 6.0, 6.0});

    // ======================== 3. 配置主从工具工件 ========================
    // 双臂协同必须填写主臂工具信息和从臂 type_s/tool_s/payload_s。
    // 当前双臂协同不支持远端工具，type 和 type_s 均设置为 0。
    ToolWorkpiece master_tw;
    master_tw.type = 0;
    master_tw.tool = {0.0, 0.0, 0.18, 0.0, 0.0, 0.0};
    master_tw.workpiece = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};

    ToolWorkpiece slave_tw;
    slave_tw.type = 0;
    slave_tw.tool = {0.0, 0.0, 0.12, 0.0, 0.0, 0.0};
    slave_tw.workpiece = master_tw.workpiece;

    ToolWorkpiece dual_tw = master_tw;
    dual_tw.type_s = slave_tw.type;
    dual_tw.tool_s = slave_tw.tool;
    dual_tw.payload_s = slave_tw.payload;

    // ======================== 4. 创建 Controller、Planner 和 Task ========================
    // 只创建一个包含 MASTER 和 SLAVE 的 Planner，后续 speedL/speedJ、避障和 moveL 均使用该 Planner。
    auto master_controller = scene->rlCreateRobotController(master_state_h, "master_controller");
    auto slave_controller = scene->rlCreateRobotController(slave_state_h, "slave_controller");
    if (!master_controller || !slave_controller) return -1;
    if ((ret = master_controller->mcInitiateController(ControlType::MOTION)) < 0) return ret;
    if ((ret = slave_controller->mcInitiateController(ControlType::MOTION)) < 0) return ret;
    if ((ret = master_controller->mcSetControlPeriod(cycle)) < 0) return ret;
    if ((ret = slave_controller->mcSetControlPeriod(cycle)) < 0) return ret;

    const PlannerHandle planner_h = scene->rlCreateRobotPlanner(
        {{master_model_h, master_state_h, PlannerRole::MASTER},
         {slave_model_h,  slave_state_h,  PlannerRole::SLAVE}},
        "dual_arm_avoid_transport_planner");
    auto planner = scene->rlGetRobotPlanner(planner_h);
    auto task = scene->rlCreateRobotTask({planner_h}, "dual_arm_avoid_transport_task");
    if (!planner || !task) return -1;
    if ((ret = task->tskSetCycle(cycle)) < 0) return ret;
    if ((ret = task->tskSetCartesianVelocityAccelerationLimits({0.2, 0.5}, {4.0, 16.0}, 0)) < 0) return ret;

    auto utility = scene->rlGetUtility();
    if (!utility) return -1;

    // ======================== 5. 准备实时反馈和控制指令缓存 ========================
    // JointCommand 由 Controller 计算，上层负责把主、从命令在同一控制周期下发。
    JointCommand master_cmd;
    JointCommand slave_cmd;

    // 上层用实际机器人反馈实时更新以下变量。
    RLJntArray master_q, master_qd, master_qdd, master_torque, master_temperature, master_friction;
    RLJntArray slave_q, slave_qd, slave_qdd, slave_torque, slave_temperature, slave_friction;

    // ======================== 6. 接入 8.3 示教结果 ========================
    // 按 8.3 文档完成起点、终点示教：STOP 后继续执行到 E_PLN_EXEC_EMPTY，
    // 再用最后一组有效主、从轨迹点生成 PathPoint。
    std::vector<PathPoint> taught_points;  // 由 8.3 示教流程填充，至少包含起点和终点。
    if (taught_points.size() < 2) return -1;

    PathPoint start = taught_points.front();
    PathPoint goal = taught_points.back();

    start.type = DescribeSpace::FULL;
    start.t_w = dual_tw;
    start.ik_strategy = IKStrategy::ROBOTIC_ARM_IK_STRATEGY;
    goal.type = DescribeSpace::FULL;
    goal.t_w = dual_tw;
    goal.ik_strategy = IKStrategy::ROBOTIC_ARM_IK_STRATEGY;

    // ======================== 7. 设置被搬运工件和环境障碍物 ========================
    // 工件附加到机器人连杆后会跟随连杆运动；环境障碍物位姿按现场实测填写。
    auto workpiece_box = std::make_shared<geometric_shapes::Box>(0.4, 0.3, 0.2);
    RLPose object_in_flange{0.0, 0.0, 0.2, 0.0, 0.0, 0.0};  // 按实际夹持关系换算。
    // 第一个参数为附加工件的父连杆；此处示例使用主臂关节 6 对应连杆。
    ret = master_model->mdlSetAttachedLink(
        "wrist3_Link", "payload_workpiece", {workpiece_box}, {object_in_flange}, {"tool0"});
    if (ret < 0) return ret;

    auto obstacle_box = std::make_shared<geometric_shapes::Box>(0.3, 0.3, 0.5);
    RLPose obstacle_world{0.3, 0.2, 0.4, 0.0, 0.0, 0.0};  // 按现场障碍物位姿填写。
    ret = utility->utlSetWorldObject("obstacle", {}, {obstacle_box}, {obstacle_world});
    if (ret < 0) return ret;

    // ======================== 8. 生成无碰绕行路点 ========================
    // 输入为用户实际示教得到的起点和终点；若直线被障碍物阻断，规划器搜索绕行点。
    ObstacleAvoidanceParameters avoidance;
    avoidance.maxDistance = 0.02;

    PathProperty avoidance_path;
    avoidance_path.describe_space = DescribeSpace::CARTESIAN;
    avoidance_path.cur_prop.type = CurveType::LINE;

    std::vector<PathPoint> free_points;
    ret = planner->tpGenerateCollisionFreePoints({start, goal}, avoidance_path, avoidance, free_points);
    if (ret < 0 || free_points.empty()) return ret < 0 ? ret : -1;

    for (auto& point : free_points) {
        if (point.type != DescribeSpace::FULL ||
            point.ik_strategy != IKStrategy::ROBOTIC_ARM_IK_STRATEGY) {
            return -1;
        }
        point.t_w = dual_tw;
    }

    // ======================== 9. 回到无碰路径首点 ========================
    // 示教终点记录后机器人通常停在终点，执行避障搬运前需先同步 moveJ 到起点。
    ret = master_state->rsInitiateRobotState(master_q, master_qd, master_qdd, master_tw);
    if (ret < 0) return ret;
    ret = slave_state->rsInitiateRobotState(slave_q, slave_qd, slave_qdd, slave_tw);
    if (ret < 0) return ret;

    master_cmd.q = master_q;
    master_cmd.qd = master_qd;
    master_cmd.qdd = master_qdd;
    master_cmd.torq = master_torque;
    slave_cmd.q = slave_q;
    slave_cmd.qd = slave_qd;
    slave_cmd.qdd = slave_qdd;
    slave_cmd.torq = slave_torque;

    ret = task->tskSetTaskState(TaskState::IDEL);
    if (ret < 0) return ret;

    PathPoint movej_start = free_points.front();
    movej_start.type = DescribeSpace::JOINT;
    movej_start.t_w = dual_tw;
    movej_start.ik_strategy = IKStrategy::NONE;

    PathProperty movej_path;
    movej_path.describe_space = DescribeSpace::JOINT;
    movej_path.cur_prop.type = CurveType::LINE;
    movej_path.blend_radius = {0.0, 0.0};

    MoveProperty movej;
    movej.move_mode = MoveMode::POSITION;
    movej.maxV = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
                  1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
    movej.maxA = {3.0, 3.0, 3.0, 3.0, 3.0, 3.0,
                  3.0, 3.0, 3.0, 3.0, 3.0, 3.0};
    movej.maxJ = {30.0, 30.0, 30.0, 30.0, 30.0, 30.0,
                  30.0, 30.0, 30.0, 30.0, 30.0, 30.0};

    ret = planner->tpAddPositionLine(movej_start, movej_path, movej);
    if (ret < 0) return ret;

    for (;;) {
        ret = master_state->rsUpdateJointState(
            master_q, master_qd, master_qdd,
            master_torque, master_temperature, master_friction);
        if (ret < 0) return ret;
        ret = slave_state->rsUpdateJointState(
            slave_q, slave_qd, slave_qdd,
            slave_torque, slave_temperature, slave_friction);
        if (ret < 0) return ret;

        std::vector<TrajectoryPoint> points;
        PlannerHandle failed_planner = -1;
        const int plan_ret = task->tskUpdateCycle(cycle, points, failed_planner);
        if (plan_ret == E_PLN_EXEC_EMPTY) break;

        const bool valid_status =
            plan_ret == E_PLN_OK || plan_ret == E_PLN_START ||
            plan_ret == E_PLN_FINISHED;
        if (!valid_status) {
            task->tskStop(StopType::EMERGENCY, 1.0, 0);
            return plan_ret;
        }

        const TrajectoryPoint* master_point = nullptr;
        const TrajectoryPoint* slave_point = nullptr;
        for (const auto& point : points) {
            if (point.id == master_model_h && !master_point) {
                master_point = &point;
            } else if (point.id == slave_model_h && !slave_point) {
                slave_point = &point;
            }
        }
        if (!master_point || !slave_point) {
            task->tskStop(StopType::EMERGENCY, 1.0, 0);
            return -1;
        }

        ret = master_state->rsUpdateReferenceTrajectory(true, *master_point);
        if (ret < 0) return ret;
        ret = slave_state->rsUpdateReferenceTrajectory(true, *slave_point);
        if (ret < 0) return ret;

        JointCommand next_master_cmd = master_cmd;
        JointCommand next_slave_cmd = slave_cmd;
        ret = master_controller->fcCalJointCommand(next_master_cmd);
        if (ret < 0) return ret;
        ret = slave_controller->fcCalJointCommand(next_slave_cmd);
        if (ret < 0) return ret;

        // 上层在同一控制周期下发 next_master_cmd 和 next_slave_cmd。
        master_cmd = next_master_cmd;
        slave_cmd = next_slave_cmd;
    }

    // 到达起点停稳后，用起点处实际反馈重新建立协同固定变换。
    ret = master_state->rsInitiateRobotState(master_q, master_qd, master_qdd, master_tw);
    if (ret < 0) return ret;
    ret = slave_state->rsInitiateRobotState(slave_q, slave_qd, slave_qdd, slave_tw);
    if (ret < 0) return ret;

    master_cmd.q = master_q;
    master_cmd.qd = master_qd;
    master_cmd.qdd = master_qdd;
    master_cmd.torq = master_torque;
    slave_cmd.q = slave_q;
    slave_cmd.qd = slave_qd;
    slave_cmd.qdd = slave_qdd;
    slave_cmd.torq = slave_torque;

    ret = task->tskSetTaskState(TaskState::IDEL);
    if (ret < 0) return ret;

    // ======================== 10. 执行协同 moveL 搬运 ========================
    // free_points.front() 已作为当前位置，从第二个无碰路点开始添加协同 moveL。
    PathProperty line_path;
    line_path.describe_space = DescribeSpace::CARTESIAN;
    line_path.cur_prop.type = CurveType::LINE;
    line_path.blend_radius = {0.01, 0.01}; // 交融会偏离原无碰折线，半径必须根据与障碍物之间的安全距离调整并低速验证。

    MoveProperty move;
    move.move_mode = MoveMode::POSITION;
    move.maxV = {0.05, 0.5};
    move.maxA = {0.2, 2.0};
    move.maxJ = {1.6, 16.0};

    planner->tpSetPlannerCapacity(PlannerCapacity::LookAhead, 1);
    for (std::size_t i = 1; i < free_points.size(); ++i) {
        ret = planner->tpAddPositionLine(free_points[i], line_path, move);
        if (ret < 0) return ret;
    }

    ret = planner->tpSetEndPath(); // 路径添加完成，触发剩余路段的速度规划。
    if (ret < 0) return ret;

    for (;;) {
        ret = master_state->rsUpdateJointState(
            master_q, master_qd, master_qdd,
            master_torque, master_temperature, master_friction);
        if (ret < 0) return ret;
        ret = slave_state->rsUpdateJointState(
            slave_q, slave_qd, slave_qdd,
            slave_torque, slave_temperature, slave_friction);
        if (ret < 0) return ret;

        std::vector<TrajectoryPoint> points;
        PlannerHandle failed_planner = -1;
        const int plan_ret = task->tskUpdateCycle(cycle, points, failed_planner);
        if (plan_ret == E_PLN_EXEC_EMPTY) break;

        const bool valid_status =
            plan_ret == E_PLN_OK || plan_ret == E_PLN_START ||
            plan_ret == E_PLN_FINISHED;
        if (!valid_status) {
            task->tskStop(StopType::EMERGENCY, 1.0, 0);
            return plan_ret;
        }

        const TrajectoryPoint* master_point = nullptr;
        const TrajectoryPoint* slave_point = nullptr;
        for (const auto& point : points) {
            if (point.id == master_model_h && !master_point) {
                master_point = &point;
            } else if (point.id == slave_model_h && !slave_point) {
                slave_point = &point;
            }
        }
        if (!master_point || !slave_point) {
            task->tskStop(StopType::EMERGENCY, 1.0, 0);
            return -1;
        }

        ret = master_state->rsUpdateReferenceTrajectory(true, *master_point);
        if (ret < 0) return ret;
        ret = slave_state->rsUpdateReferenceTrajectory(true, *slave_point);
        if (ret < 0) return ret;

        JointCommand next_master_cmd = master_cmd;
        JointCommand next_slave_cmd = slave_cmd;
        ret = master_controller->fcCalJointCommand(next_master_cmd);
        if (ret < 0) return ret;
        ret = slave_controller->fcCalJointCommand(next_slave_cmd);
        if (ret < 0) return ret;

        // 上层在同一控制周期下发 next_master_cmd 和 next_slave_cmd。
        // E_PLN_FINISHED 也需下发，下一周期返回 E_PLN_EXEC_EMPTY 才退出。
        master_cmd = next_master_cmd;
        slave_cmd = next_slave_cmd;
    }

    // ======================== 11. 清理场景 ========================
    // 删除本次 Demo 添加的世界障碍物和附加工件，避免影响后续规划任务。
    utility->utlUnsetWorldObject("obstacle");
    master_model->mdlRemoveAttachedLink("payload_workpiece");

    return E_PLN_OK;
}

6. 参数说明

6.1 ObstacleAvoidanceParameters

struct ObstacleAvoidanceParameters
{
    ObstacleAvoidanceAlgorithm avoid_type{ObstacleAvoidanceAlgorithm::RRTCONNECT};
    double maxDistance;
    bool simplify{true};
};

• avoid_type：双臂协同避障内部默认使用 RRT，用户无需显式指定，Demo 中不对该字段赋值。
• maxDistance：规划步长，应结合障碍物尺度和场景空间设置。过大会增加漏检风险，过小会降低规划效率。
• simplify：双臂笛卡尔协同避障中无需显式指定该字段，手动设置会影响搜索效率，Demo 中不对该字段赋值。

6.2 运动参数

speedL 的 tarV/maxV/maxA/maxJ 为 6 维协同对象笛卡尔分量：

[vx, vy, vz, wx, wy, wz]

speedJ 的速度、加速度、加加速度按主、从关节顺序填写。协同 moveL 的 maxV/maxA/maxJ 为 2 维平移/旋转模长约束：

move.maxV = {0.05, 0.5};   // [最大线速度, 最大角速度]
move.maxA = {0.2, 2.0};    // [最大线加速度, 最大角加速度]
move.maxJ = {1.6, 16.0};   // [最大线加加速度, 最大角加加速度]

7. 输出说明

tpGenerateCollisionFreePoints() 输出的是绕行后的无碰路点序列：

• 输出路点应为 DescribeSpace::FULL，同时携带协同对象位姿 pose 与主、从关节构型 joint_m/joint_s；
• 输出序列包含起点、终点和必要的中间绕行点；
• 执行阶段先同步 moveJ 到第一个输出点，也就是起点；到位并重新进入 IDEL 后，再从第二个输出点开始逐点调用 tpAddPositionLine() 添加协同 moveL；
• Demo 中不对输出点或执行取点循环重复做碰撞检测，避免影响取点效率。

8. 注意事项

1. 起点和终点必须来自用户实际示教，不能在 Demo 中手动构造协同对象位姿替代示教。
2. 示教停止后必须等 tskUpdateCycle() 返回 E_PLN_EXEC_EMPTY，再记录最后一组有效主、从轨迹点。
3. 更换 TCP、夹持位置或工件尺寸后，必须重新初始化并重新示教。
4. touch_links 只用于屏蔽工件与夹持连杆的正常接触，不应扩大忽略范围。
5. 规划失败时，应调整障碍物建模、起终点、夹持姿态或 maxDistance 后重新规划。
6. 使用交融时，必须复核交融轨迹与障碍物之间的安全距离；不能因为原始折线无碰就默认交融后仍然无碰。
7. 搬运结束后调用 utlUnsetWorldObject() 和 mdlRemoveAttachedLink() 恢复场景。

9. 相关文档

• 双臂协同示教与搬运使用说明
• 双臂/外部轴关节同步避障使用说明
• 避障规划介绍及使用实例